using EarthUtility;
using Unity.Mathematics;
namespace EarthGeospatial
{
    /**
     * @brief Transforms positions to various reference frames.
     */
    public class Transforms
    {
        /**
         * @brief Computes a transformation from east-north-up axes to an
         * ellipsoid-fixed reference frame.
         *
         * Computes a 4x4 transformation matrix from a reference frame with an
         * east-north-up axes centered at the provided origin to the provided
         * ellipsoid's fixed reference frame. The local axes are defined as: <ul>
         *  <li>The `x` axis points in the local east direction.</li>
         *  <li>The `y` axis points in the local north direction.</li>
         *  <li>The `z` axis points in the direction of the ellipsoid surface normal
         * which passes through the position.</li>
         * </ul>
         *
         * @param origin The center point of the local reference frame.
         * @param ellipsoid The {@link Ellipsoid} whose fixed frame is used in the
         * transformation. Default value: {@link Ellipsoid::WGS84}.
         * @return The transformation matrix
         */
       public static double4x4 eastNorthUpToFixedFrame(double3 origin) => eastNorthUpToFixedFrame(origin, Ellipsoid.WGS84);
       public  static double4x4 eastNorthUpToFixedFrame(double3 origin, Ellipsoid ellipsoid)
        {
            if (Mathd.equalsEpsilon(origin, new double3(0.0), Mathd.EPSILON14))
            {
                // If x, y, and z are zero, use the degenerate local frame, which is a
                // special case
                return new double4x4(
                    new double4(0.0, 1.0, 0.0, 0.0),
                    new double4(-1.0, 0.0, 0.0, 0.0),
                    new double4(0.0, 0.0, 1.0, 0.0),
                    new double4(origin, 1.0));
            }
            if (Mathd.equalsEpsilon(origin.x, 0.0, Mathd.EPSILON14) &&
                Mathd.equalsEpsilon(origin.y, 0.0, Mathd.EPSILON14))
            {
                // If x and y are zero, assume origin is at a pole, which is a special case.
                double sign = math.sign(origin.z);
                return new double4x4(
                    new double4(0.0, 1.0, 0.0, 0.0),
                    new double4(-1.0 * sign, 0.0, 0.0, 0.0),
                    new double4(0.0, 0.0, 1.0 * sign, 0.0),
                    new double4(origin, 1.0));
            }

            double3 up = ellipsoid.geodeticSurfaceNormal(origin);
            double3 east = math.normalize(new double3(-origin.y, origin.x, 0.0));
            double3 north = math.cross(up, east);

            return new double4x4(
                new double4(east, 0.0),
                new double4(north, 0.0),
                new double4(up, 0.0),
                new double4(origin, 1.0));
        }


    }
}